//
// Created by 13562 on 2024/1/3.
//

#include "ImagePublisher.h"

ImagePublisher::ImagePublisher(const ros::NodeHandle &nodeHandle, ImageTopics imageTopics) : imageTransport(nodeHandle) {
	imagePublisher = imageTransport.advertise(imageTopics.originImageTopic, 1);
	leftPublisher = imageTransport.advertiseCamera(imageTopics.leftImageTopic, 1);
	rightPublisher = imageTransport.advertiseCamera(imageTopics.rightImageTopic, 1);
	cameraInfoManager = boost::shared_ptr<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(nodeHandle));

//	读取服务器参数，获得左右相机参数文件位置
	string leftCalFile = nodeHandle.param<std::string>("left_cam_file", "");
	string rightCalFile = nodeHandle.param<std::string>("right_cam_file", "");
	if (!leftCalFile.empty()) {
		if (cameraInfoManager->validateURL(leftCalFile)) {
			cout << "Load left camera info file: " << leftCalFile << endl;
			cameraInfoManager->loadCameraInfo(leftCalFile);
			leftCameraInfoPtr = sensor_msgs::CameraInfoPtr(
					new sensor_msgs::CameraInfo(cameraInfoManager->getCameraInfo()));
		} else {
			cout << "Can't load left camera info of file: " << leftCalFile << endl;
			ros::shutdown();
		}
	} else {
		cout << "Did not specify left camera info file. " << endl;
		leftCameraInfoPtr = sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo());
	}
	if (!rightCalFile.empty()) {
		if (cameraInfoManager->validateURL(rightCalFile)) {
			cout << "Load right camera info file: " << rightCalFile << endl;
			cameraInfoManager->loadCameraInfo(rightCalFile);
			rightCameraInfoPtr = sensor_msgs::CameraInfoPtr(
					new sensor_msgs::CameraInfo(cameraInfoManager->getCameraInfo()));
		} else {
			cout << "Can't load right camera info of file: " << rightCalFile << endl;
			ros::shutdown();
		}
	} else {
		cout << "Did not specify right camera info file. " << endl;
		rightCameraInfoPtr = sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo());
	}

}

void ImagePublisher::split(cv::Mat &left, cv::Mat &right) {
	if (origin.empty()) cout << "in func split(): split origin empty." << endl;
	int midCol = origin.cols / 2;
	int height = origin.rows;
	left = origin(cv::Rect(0, 0, midCol, height));
	right = origin(cv::Rect(midCol, 0, midCol, height));
}

void ImagePublisher::split() {
	split(leftImage, rightImage);
}

bool ImagePublisher::setNewFrame(const cv::Mat &frame) {
	origin = frame.clone();
	return !origin.empty();
}

/**
 * 发布合并图像Topic，被动更新，由ros::_ok()驱动
 * @param imageCatcher
 * @return
 */
sensor_msgs::ImagePtr ImagePublisher::publishComposite(ImageCatcher imageCatcher) {
	setNewFrame(imageCatcher.getFrame());
	sensor_msgs::ImagePtr imagePtr = cv_bridge::CvImage(std_msgs::Header(), "bgr8", origin).toImageMsg();
	imagePublisher.publish(imagePtr);
	return imagePtr;
}

/**
 * 发布全部图像Topic，包括composite, left, right
 * 被动更新，由ros::_ok()驱动
 * @param imageCatcher
 */
void ImagePublisher::publishAll(const ImageCatcher& imageCatcher) {
	// composite在此处发布，publishComposite返回一个ImagePtr
	sensor_msgs::ImagePtr imagePtr = publishComposite(imageCatcher);
	// split主要是将合并图像分解为左右图像，并存到leftImage和rightImage中
	split();
	leftImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(std_msgs::Header(), "bgr8", leftImage));
	rightImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(std_msgs::Header(), "bgr8", rightImage));
	cv_bridge::CvImageConstPtr cv_ptr;
	cv_ptr = cv_bridge::toCvShare(imagePtr, "bgr8");
	leftCameraInfoPtr->header = cv_ptr->header;
	rightCameraInfoPtr->header = cv_ptr->header;
	sensor_msgs::ImagePtr leftPtr = leftImagePtr->toImageMsg();
	sensor_msgs::ImagePtr rightPtr = rightImagePtr->toImageMsg();
	leftPtr->header = imagePtr->header;
	rightPtr->header = imagePtr->header;
	leftPublisher.publish(leftPtr, leftCameraInfoPtr);
	rightPublisher.publish(rightPtr, rightCameraInfoPtr);
}

/**
 * 订阅合并图像Topic，通过回调来进行转发
 * @param compositeTopic
 */
void ImagePublisher::republish(const char* compositeTopic) {
	imageSubscriber = imageTransport.subscribe(compositeTopic, 1, &ImagePublisher::imageCallback, this);
	cout << "in func republish(compositeTopic): " << "subscribed composite topic." << endl;
}

/**
 * publish的回调函数
 * @param msg
 */
void ImagePublisher::imageCallback(const sensor_msgs::ImageConstPtr &msg) {
	cv_bridge::CvImageConstPtr cv_ptr;
	cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
	if (cv_ptr->image.empty()) cout << "in imageCallback(): msg image empty" << endl;
	this->origin = cv_ptr->image;
	split();
	leftImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(std_msgs::Header(), "bgr8", leftImage));
	rightImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(std_msgs::Header(), "bgr8", rightImage));
	leftCameraInfoPtr->header = cv_ptr->header;
	rightCameraInfoPtr->header = cv_ptr->header;
	sensor_msgs::ImagePtr leftPtr = leftImagePtr->toImageMsg();
	sensor_msgs::ImagePtr rightPtr = rightImagePtr->toImageMsg();
	leftPtr->header = msg->header;
	rightPtr->header = msg->header;
	leftPublisher.publish(leftPtr, leftCameraInfoPtr);
	rightPublisher.publish(rightPtr, rightCameraInfoPtr);
	cv::imshow("origin", origin);
}

/**
 * 由Stream Factory获取帧并发布
 * 被动更新，由ros::_ok()驱动
 * @param streamFactory
 */
void ImagePublisher::publishStream(const StreamFactory &streamFactory) {
//	读取Mat
	leftImage = streamFactory.getCurrLeftFrame();
	rightImage = streamFactory.getCurrRightFrame();
	cv::cvtColor(leftImage, leftImage, cv::COLOR_BGR2GRAY);
	cv::cvtColor(rightImage, rightImage, cv::COLOR_BGR2GRAY);
//	设置msg header
	std_msgs::Header header;
	header.seq = streamFactory.getCurrFrameIndex();
	uint64_t ns = streamFactory.getCurrTimestamp();
	chrono::nanoseconds t(ns);
	chrono::seconds secs = chrono::duration_cast<chrono::seconds>(t);
	chrono::nanoseconds nsecs = t - chrono::duration_cast<chrono::nanoseconds>(chrono::seconds(secs.count()));
	header.stamp.sec = secs.count();
	header.stamp.nsec = nsecs.count();

	header.frame_id = to_string(streamFactory.getCurrFrameIndex());
//	header.stamp = ros::Time::now();
//	构造ImagePtr
	leftImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(header, "mono8", leftImage));
	rightImagePtr = cv_bridge::CvImagePtr(new cv_bridge::CvImage(header, "mono8", rightImage));
//	构造CameraInfoPtr， CameraInfo中相机参数不设置，后续程序校正
	leftCameraInfoPtr->header = header;
	rightCameraInfoPtr->header = header;
//	发布双目图像
	leftPublisher.publish(leftImagePtr->toImageMsg(), leftCameraInfoPtr);
	rightPublisher.publish(rightImagePtr->toImageMsg(), rightCameraInfoPtr);
}
